/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package com.nurdrobotics;

import edu.wpi.first.wpilibj.*;

/**
 *
 */
public class Drivetrain {
    private static RobotDrive m_drive = null; // drive object used to drive the robot

    /*
     * Note, we can't use a static initializer because the robot drive object
     * would be created as soon as the program starts. However, because the
     * motors are CAN bus motors, then we will get timeouts.
     *
     * So after the first time getRobotDrive is called, you need to start
     * calling updates on the robotDrive object (such as arcadeDrive, etc)
     *
     * We need to find a way to disable that safety feature of the robot drive
     * because there will probably be times after we call getRobotDrive where
     * we will be idle for a while.
     */
    private static RobotDrive getRobotDrive() {
        if(m_drive == null) {
            // create a robot drive system that consist of four motors
            m_drive = new RobotDrive(
                    Motor.getInstance(Motor.ID_DRIVE_FRONT_LEFT),
                    Motor.getInstance(Motor.ID_DRIVE_REAR_LEFT),
                    Motor.getInstance(Motor.ID_DRIVE_FRONT_RIGHT),
                    Motor.getInstance(Motor.ID_DRIVE_REAR_RIGHT));

            m_drive.setSafetyEnabled(false);
        }

        return m_drive;
    }

    /*
     * This routine drives the robot according the specified move and rotate
     * values
     */
    public static void drive(double moveValue, double rotateValue) {
        RobotDrive drive = getRobotDrive();
        drive.arcadeDrive(moveValue, rotateValue);
    }

    /**
     * This function is called continuously while in teleoperated mode to
     * update the state of the drivetrain
     */
    public static void teleopUpdate() {
        // Drive in arcade drive using the driver joystick as inputs
        drive(DriverStick.getMoveValue(), DriverStick.getRotateValue());
    }
}
